#include <math.h>
#include "zuc/nml_intf/zucpos.h"
#include "rcs_print.hh"
#include "kinematics/kinematics.h"
#include <stdio.h>
#include <string.h>
#include <stdio.h>
#include <time.h>
#ifndef TRUE
#define TRUE 1
#endif

#ifndef FALSE
#define FALSE 0
#endif

#define SINGULAR_FUZZ 0.000001
#define FLAG_FUZZ 0.000001
#define REFRESH 500

/* flags for inverse kinematics */
#define JAKA_SHOULDER_RIGHT 0x01
#define JAKA_ELBOW_DOWN 0x02
#define JAKA_WRIST_FLIP 0x04
#define JAKA_SINGULAR 0x08 /* joints at a singularity */

/* flags for forward kinematics */
#define JAKA_UNREACH 0x01 /* pose out of reach */

int setInterpDHParam(DHParam dhparam)
{
    printf("jakar7kin_interp dh param: %f, %f, %f, %f, %f, %f\n", dhparam.a[0], dhparam.a[1], dhparam.a[2], dhparam.a[3], dhparam.a[4], dhparam.a[5]);
    // updateKineDHParams(dhparam);
    return 0;
}

// int setKineRobotSeries(int series)
// {
//     printf("[JAKAR7KIN_INTERP]>>>>>>>>>>>>>robot serial: %d\n", series);
//     // robot_series = series;
//     setKineRobotSeries(series);
//     return 0;
// }

// int setFullDHCom(int full_dh_com)
// {
//     printf("[JAKAR7KIN_INTERP]>>>>>>>>>>>>>full_dh_com: %d\n", full_dh_com);
//     // robot_fulldhcom = full_dh_com;
//     setFullDHCom(full_dh_com);
//     return 0;
// }

#define TP_MIN_ARC_ANGLE 1e-3

int sat_inplace(double* const x, double max)
{
    if (*x > max)
    {
        *x = max;
        return 1;
    }
    else if (*x < -max)
    {
        *x = -max;
        return -1;
    }
    return 0;
}
PmCartesian threePointCircle(PmCartesian A, PmCartesian B, PmCartesian C)
{
    PmCartesian u, v;    //求出的新坐标系X,Y方向上的向量
    PmCartesian center;  //圆心
                         //  PmCartesian AB, AC;	 // AB: 向量AB， AC：向量AC
    PmCartesian ex, ey;  //ex: 新坐标系X轴单位向量 ，ey: 新坐标系Y轴单位向量
    PmCartesian vy, vx;  // C点做垂先和AB相交，所得的X轴上的向量。
    double xB, xC, yC;   // AB的长度。

    PmCartesian tempEx, tempEy;
    //计算 u = B - A;
    pmCartCartSub(&B, &A, &u);  //计算 v = C - A;
    pmCartCartSub(&C, &A, &v);  //计算 xB = norm(u)
    pmCartMag(&u, &xB);         //计算 ex = u/xB
    pmCartUnit(&u, &ex);        //vx = （v * ex）*ex;
    pmCartCartDot(&v, &ex, &xC);
    pmCartScalMult(&ex, xC, &vx);  //计算 vy = v - vx;
    pmCartCartSub(&v, &vx, &vy);   //计算Y轴单位向量 ey = ey/|ey|
    pmCartUnit(&vy, &ey);          //计算 yC = |vy|
    pmCartMag(&vy, &yC);           //计算center = A + 1/2 xB*ex + (-xB (xB - xC) xC + xB yC^2)/(2 xB yC)*ey
    pmCartScalMult(&ex, 0.5 * xB, &tempEx);
    pmCartCartAdd(&A, &tempEx, &tempEx);
    pmCartScalMult(&ey, (-1 * xB * (xB - xC) * xC + xB * yC * yC) / (2 * xB * yC), &tempEy);
    pmCartCartAdd(&tempEx, &tempEy, &center);

    return center;
}

int getMovcEnd(ZucPose const* const start, ZucPose const* const mid, ZucPose const* const end, double count, ZucPose* out)
{
    //计算圆心
    PmCartesian startPosition;
    PmCartesian midPosition;
    PmCartesian endPosition;

    PmCartesian center;
    PmCartesian uCenterStart;
    PmCartesian uCenterMid;
    PmCartesian uCenterEnd;
    PmCartesian normal, normal1, normal2;

    double radius;
    double theta, scm, mce, sce;
    // double thetaMD;
    double startEndDot;
    double startMidDot;
    double midEndDot;
    PmRotationMatrix rotStart;
    PmRotationMatrix rotEnd;
    PmRotationMatrix rot;
    PmRotationMatrix startInv;
    PmRpy rpyStart, rpyEnd;
    PmRotationVector abc;
    double normalMag;
    double cosTheta;
    // int turn;
    int use_sm = 1;
    startPosition = start->tran;
    midPosition = mid->tran;
    endPosition = end->tran;

    // printf(" --------pmMovcInit------- : startPosition =%f   %f  %f  \n  ",startPosition.x,startPosition.y,startPosition.z);
    // printf(" --------pmMovcInit------- : midPosition =%f   %f  %f \n ",midPosition.x,midPosition.y,midPosition.z);
    // printf(" --------pmMovcInit------- : end =%f   %f  %f  \n ",endPosition.x,endPosition.y,endPosition.z);

    center = threePointCircle(startPosition, midPosition, endPosition);
    //printf(" --------pmMovcInit------- : center =%f   %f  %f  \n  ",center.x,center.y,center.z);

    pmCartCartSub(&startPosition, &center, &uCenterStart);
    pmCartCartSub(&midPosition, &center, &uCenterMid);
    pmCartCartSub(&endPosition, &center, &uCenterEnd);

    //printf("uCenterStart = %f %f %f \n ",uCenterStart.x,uCenterStart.y,uCenterStart.z);
    //printf("uCenterMid = %f %f %f \n ",uCenterMid.x,uCenterMid.y,uCenterMid.z);
    //printf("uCenterMid = %f %f %f \n ",uCenterEnd.x,uCenterEnd.y,uCenterEnd.z);

    //计算半径
    pmCartMag(&uCenterStart, &radius);
    //printf(" --------pmMovcInit------- : radius =%f   \n  ",radius);

    //计算旋转方向
    //计算圆心角
    pmCartCartDot(&uCenterStart, &uCenterEnd, &startEndDot);
    cosTheta = startEndDot / radius / radius;
    sat_inplace(&cosTheta, 1);
    theta = acos(cosTheta);

    //printf(" --------pmMovcInit------- : theta =%f   \n  ",theta);

    //计算姿态变化，
    if (fabs(theta) < TP_MIN_ARC_ANGLE)
    {
        return -1;
    }
    //计算法向量
    pmCartCartDot(&uCenterStart, &uCenterMid, &startMidDot);
    scm = acos(startMidDot / radius / radius);

    pmCartCartDot(&uCenterMid, &uCenterEnd, &midEndDot);
    mce = acos(midEndDot / radius / radius);

    pmCartCartDot(&uCenterStart, &uCenterEnd, &startEndDot);
    sce = acos(startEndDot / radius / radius);

    //printf(" --------pmMovcInit------- : scm =%f   mce =%f \n  ",scm,mce);

    //printf(" --------pmMovcInit------- : uCenterStart =%f   %f  %f  \n  ",uCenterStart.x,uCenterStart.y,uCenterStart.z);
    //printf(" --------pmMovcInit------- : uCenterMid =%f   %f  %f \n ",uCenterMid.x,uCenterMid.y,uCenterMid.z);
    //printf(" --------pmMovcInit------- : uCenterEnd =%f   %f  %f  \n ",uCenterEnd.x,uCenterEnd.y,uCenterEnd.z);

    // if(fabs(theta - PM_PI)>TP_MIN_ARC_ANGLE){
    // 	pmCartCartCross(&uCenterStart, &uCenterEnd, &normal);
    // }
    // if (fabs(scm - PM_PI) > TP_MIN_ARC_ANGLE)
    // {
    //     pmCartCartCross(&uCenterStart, &uCenterMid, &normal);
    // }
    // else if (fabs(mce - PM_PI) > TP_MIN_ARC_ANGLE)
    // {
    //     pmCartCartCross(&uCenterMid, &uCenterEnd, &normal);
    //     use_sm = 0;
    // }
    // else
    // {
    //     return 0;
    // }
    // pmCartMag(&normal, &normalMag);

    // if (normalMag < TP_MIN_ARC_ANGLE)
    // {
    //     return 0;
    // }
    // pmCartMag(&normal, &normalMag);

    // normal.x /= normalMag;
    // normal.y /= normalMag;
    // normal.z /= normalMag;

    // // 计算旋转方向
    // pmCartCartCross(&uCenterStart, &uCenterMid, &normal1);
    // pmCartCartCross(&uCenterStart, &uCenterEnd, &normal2);
    // //printf("normal1 = %f %f %f \n ",normal1.x,normal1.y,normal1.z);
    // //printf("normal2 = %f %f %f \n ",normal2.x,normal2.y,normal2.z);
    // pmCartUnitEq(&normal1);
    // pmCartUnitEq(&normal2);
    // if (fabs(normal1.x - normal2.x) < TP_MIN_ARC_ANGLE && fabs(normal1.y - normal2.y) < TP_MIN_ARC_ANGLE && fabs(normal1.z - normal2.z) < TP_MIN_ARC_ANGLE)
    // {
    //     //
    //     if (scm > sce)
    //     {
    //         if (use_sm)
    //         {
    //             pmCartCartCross(&uCenterMid, &uCenterStart, &normal);
    //         }
    //         else
    //         {
    //             pmCartCartCross(&uCenterEnd, &uCenterMid, &normal);
    //         }
    //         theta = PM_2_PI - theta;
    //     }
    // }
    // else
    // {
    //     theta = PM_2_PI - theta;
    // }
    // pmCartUnitEq(&normal);
    //printf(" --------pmMovcInit------- : normal =%f   %f  %f  \n  ",normal.x,normal.y,normal.z);

    ////* position major/minor arc
    ////* 计算旋转方向
    if (fabs(sce - PM_PI) < TP_MIN_ARC_ANGLE)
    {
        if (fabs(scm - PM_PI) < TP_MIN_ARC_ANGLE)
        {
            printf("*****get Movc end failed - invalid mid and end position: start_end angle: %lf, start_mid angle: %lf*****\n", sce, scm);
            return -1;
        }
    }
    pmCartCartCross(&uCenterStart, &uCenterEnd, &normal);
    pmCartUnitEq(&normal);

    if (fabs(sce - scm - mce) > TP_MIN_ARC_ANGLE)
    {
        normal.x = -normal.x;
        normal.y = -normal.y;
        normal.z = -normal.z;
        theta = PM_2_PI - theta;
    }

    rpyStart.y = start->a * PM_PI / 180.0;
    rpyStart.p = start->b * PM_PI / 180.0;
    rpyStart.r = start->c * PM_PI / 180.0;

    rpyEnd.y = end->a * PM_PI / 180.0;
    rpyEnd.p = end->b * PM_PI / 180.0;
    rpyEnd.r = end->c * PM_PI / 180.0;

    pmRpyMatConvert(&rpyStart, &rotStart);
    pmRpyMatConvert(&rpyEnd, &rotEnd);

    pmMatInv(&rotStart, &startInv);
    // pmMatMatMult(&startInv, &rotEnd,&rot);
    pmMatMatMult(&rotEnd, &startInv, &rot);

    pmMatRotConvert(&rot, &abc);

    //printf(" --------pmMovcInit------- : abc = %f   %f   %f    %f   \n  ",abc.s,abc.x,abc.y,abc.z);

    //	circle->abc = abc;
    //	circle->normal = normal;
    //	circle->radius = radius;
    //	circle->start.tran =  startPosition;
    //	circle->start.rot = rotStart;
    //	circle->end.tran =  endPosition;
    //	circle->end.rot = rotEnd;
    //	circle->center = center;
    //circle->turn = 1;
    //	circle->angle = theta;
    theta = count * PM_PI * 2;

    double k;
    //printf(" -------tcGetPosReal------- :progress = %f  target = %f \n	",progress,tc->target);
    k = theta;  //圆心角(弧度) * 当前弧长/总弧长
    PmRotationVector rotK;
    //	PmRotationMatrix rot;
    //	PmRotationMatrix rotEnd;
    //	PmRpy rpyEnd;
    rotK.s = k;
    rotK.x = normal.x;
    rotK.y = normal.y;
    rotK.z = normal.z;
    PmCartesian p1, p2;

    pmCartCartSub(&start->tran, &center, &p1);
    pmRotMatConvert(&rotK, &rot);
    //计算插补点xyz
    //printf(" -------tcGetPosReal------- :rot.x = %f	 %f  %f  \n   ",rot.x.x,rot.x.y,rot.x.z);
    //printf(" -------tcGetPosReal------- :rot.y = %f	 %f  %f  \n   ",rot.y.x,rot.y.y,rot.y.z);
    //printf(" -------tcGetPosReal------- :rot.z = %f	 %f  %f  \n   ",rot.z.x,rot.z.y,rot.z.z);
    //printf(" -------tcGetPosReal------- :p1 = %f   %f  %f  \n   ",p1.x,p1.y,p1.z);

    pmMatCartMult(&rot, &p1, &p2);

    pmCartCartAdd(&p2, &center, &out->tran);
    //printf(" -------tcGetPosReal------- :xyz = %f   %f  %f  \n	",xyz.x,xyz.y,xyz.z);

    pmMatMatMult(&rot, &rotStart, &rotEnd);
    pmMatRpyConvert(&rotEnd, &rpyEnd);
    //printf("  \n   ");
    //printf(" -------tcGetPosReal------- :rot.x = %f	 %f  %f  \n   ",rot.x.x,rot.x.y,rot.x.z);
    //printf(" -------tcGetPosReal------- :rot.y = %f	 %f  %f  \n   ",rot.y.x,rot.y.y,rot.y.z);
    //printf(" -------tcGetPosReal------- :rot.z = %f	 %f  %f  \n   ",rot.z.x,rot.z.y,rot.z.z);

    //printf(" -------tcGetPosReal------- :rotEnd.x = %f	 %f  %f  \n   ",rotEnd.x.x,rotEnd.x.y,rotEnd.x.z);
    //printf(" -------tcGetPosReal------- :rotEnd.y = %f	 %f  %f  \n   ",rotEnd.y.x,rotEnd.y.y,rotEnd.y.z);
    //printf(" -------tcGetPosReal------- :rotEnd.z = %f	 %f  %f  \n   ",rotEnd.z.x,rotEnd.z.y,rotEnd.z.z);

    out->a = rpyEnd.y * 180.0 / PM_PI;
    out->b = rpyEnd.p * 180.0 / PM_PI;
    out->c = rpyEnd.r * 180.0 / PM_PI;
    return 0;
}

int getToolMovPos(const ZucPose* world, const ZucPose* pos, ZucPose* out)
{
    PmHomogeneous t_world, t_pos, t_out;
    PmRpy rpy;
    t_world.tran = world->tran;
    t_pos.tran = pos->tran;
    rpy.y = world->a * PM_PI / 180.0;
    rpy.p = world->b * PM_PI / 180.0;
    rpy.r = world->c * PM_PI / 180.0;
    pmRpyMatConvert(&rpy, &t_world.rot);

    printf(" world = %f %f %f  %f %f %f \n ", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c);
    printf(" pos = %f %f %f  %f %f %f \n ", pos->tran.x, pos->tran.y, pos->tran.z, pos->a, pos->b, pos->c);

    rpy.y = pos->a * PM_PI / 180.0;
    rpy.p = pos->b * PM_PI / 180.0;
    rpy.r = pos->c * PM_PI / 180.0;
    pmRpyMatConvert(&rpy, &t_pos.rot);

    pmHomHomMult(&t_world, &t_pos, &t_out);
    out->tran = t_out.tran;
    pmMatRpyConvert(&t_out.rot, &rpy);
    out->a = rpy.y * 180.0 / PM_PI;
    out->b = rpy.p * 180.0 / PM_PI;
    out->c = rpy.r * 180.0 / PM_PI;
    printf(" out = %f %f %f  %f %f %f \n ", out->tran.x, out->tran.y, out->tran.z, out->a, out->b, out->c);
    return 0;
}

// #define GET_SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
// #define GET_MAX(a, b) (a > b ? a : b)
// #define GET_MIN(a, b) (a < b ? a : b)
// #define PINV_MAX 10
// double u[PINV_MAX * PINV_MAX] = {0};
// double w[PINV_MAX] = {0};
// double vw[PINV_MAX * PINV_MAX] = {0};
// double v[PINV_MAX * PINV_MAX] = {0};
// double uT[PINV_MAX * PINV_MAX] = {0};